#include "ros/ros.h"
#include "ros/package.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <queue>
#include <opencv2/opencv.hpp>
//#include <opencv2/imgcodecs.hpp>
//#include <opencv2/imgproc.hpp>
//#include <opencv2/videoio.hpp>
//#include <opencv2/highgui.hpp>
//#include <opencv2/video.hpp>
//#include <vector>
//#include <opencv2/core.hpp>
//#include <opencv2/calib3d.hpp>


#define CAP_INDEX 0

using namespace cv;
using namespace std;

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "drone_pnp_node");
    ros::NodeHandle n;
//    Subscriber pos_sub=n.subscribe("/mocap/pose",10,ViconPosCallback);
//    ros::Publisher pos_pub = n.advertise<geometry_msgs::PoseStamped>("/cam/pose", 10);

    ros::Rate loop_rate(30);

    //相机参数
    Mat cameraMatrix;
    cameraMatrix = Mat::zeros(3, 3, CV_64F);
    const vector<double> distCoeffs{0.03819613038058793, -0.06112508916119349, -5.515361356058103e-05,
                                    0.003470678820019871, 0};

    cameraMatrix.at<double>(0, 0) = 411.1663471332097;
    cameraMatrix.at<double>(0, 2) = 331.9546551659722;
    cameraMatrix.at<double>(1, 1) = 411.4132376251969;
    cameraMatrix.at<double>(1, 2) = 268.2985302141446;
    cameraMatrix.at<double>(2, 2) = 1;

    //! [capture]
    VideoCapture capture(CAP_INDEX);
    capture.set(CAP_PROP_AUTO_EXPOSURE, 0.75); //0.75 auto on, 0.25 auto off
//    capture.set(CAP_PROP_EXPOSURE, 0.01);

    if (!capture.isOpened()) {
        //error in opening the video input
        cerr << "Unable to open: " << CAP_INDEX << endl;
        return 0;
    }
    //! [capture]

    std::string pkg_path = ros::package::getPath("drone_pnp");
    Mat frame,img,hsvImg,valueImg;

    while (ros::ok()) {
        capture >> frame;
        if (frame.empty())
            break;
//        undistort(frame, img, cameraMatrix, distCoeffs);
//        imshow("undistorted", img);
//
//        cvtColor(img, hsvImg, CV_BGR2HSV);
//
//        for (int i = 0; i < img.rows; i++) {
//            for (int j = 0; j < img.cols; j++) {
//                const int hi = i * img.cols * 3 + j * 3,
//                        gi = i * img.cols + j;
//                valueImg.data[gi] = hsvImg.data[hi + 2];
//            }
//        }
//
////        threshold(frame_masked, binImg, 200, 255, THRESH_BINARY);

        imshow("Frame", frame);

        waitKey(1);





        ros::spinOnce();
//        loop_rate.sleep();
    }
    return 0;
}
